#include "LevelDriver.h"
#include "DSP2833x_Device.h"
#include "UARTDriverA.h"
#include <stdio.h>
#include <math.h>

extern UARTDriverA uartDriverA;

void LevelDriver::Init()
{
	EALLOW;
	// 1. GPIO
	GpioCtrlRegs.GPBPUD.bit.GPIO54 = 0;   // Enable pull-up on GPIO54 (SPISIMOA)
    GpioCtrlRegs.GPBPUD.bit.GPIO55 = 0;   // Enable pull-up on GPIO55 (SPISOMIA)
    GpioCtrlRegs.GPBPUD.bit.GPIO56 = 0;   // Enable pull-up on GPIO56 (SPICLKA)
    GpioCtrlRegs.GPBQSEL2.bit.GPIO54 = 3; // Asynch input GPIO54 (SPISIMOA)
	GpioCtrlRegs.GPBQSEL2.bit.GPIO55 = 3; // Asynch input GPIO55 (SPISOMIA)
	GpioCtrlRegs.GPBQSEL2.bit.GPIO56 = 3; // Asynch input GPIO56 (SPICLKA)
	GpioCtrlRegs.GPBMUX2.bit.GPIO54 = 1; // Configure GPIO54 as SPISIMOA
	GpioCtrlRegs.GPBMUX2.bit.GPIO55 = 1; // Configure GPIO55 as SPISOMIA
	GpioCtrlRegs.GPBMUX2.bit.GPIO56 = 1; // Configure GPIO56 as SPICLKA

	GpioCtrlRegs.GPBPUD.bit.GPIO52 = 0;   // Enable pull-up on GPIO52 (LEVELCS)
	GpioCtrlRegs.GPBMUX2.bit.GPIO52 = 0; // configure LEVELCS as GPIO
	GpioDataRegs.GPBSET.bit.GPIO52 = 1; // deselect
	GpioCtrlRegs.GPBDIR.bit.GPIO52 = 1; // configure LEVELCS as output
	
	EDIS;

	SetupSPI();

	// check code
	WriteData(Ctrl_Reg1, 0xD7); // 160Hz
	WriteData(Ctrl_Reg2, 0x40);
	unsigned char code = ReadData(WhoAmI);
	char tmp[100];
	sprintf(tmp, "Level Code: %2X\r\n", code);
	uartDriverA.SendData(tmp);

	if( code !=  0x3A )
	{
	// error
	}
}

void LevelDriver::SetupSPI()
{
	// configure SPI, clock =  Clock = 37.5Mhz/5 = 7.5Mhz, 16 bit, Master Mode (NOTE: Can be run at 8 Mhz!)
	SpiaRegs.SPICCR.all =0x004F;	             // Reset on, falling edge (polarity = 1), 16-bit char bits  
	SpiaRegs.SPICTL.all =0x0006;    		     // Enable master mode, normal phase (phase = 0), enable talk, and SPI int disabled.	
	SpiaRegs.SPIBRR =0x0004;	// 37.5MHz/5											
    SpiaRegs.SPICCR.all =0x00CF;		         // Relinquish SPI from Reset   
    SpiaRegs.SPIPRI.bit.FREE = 1;                // Set so breakpoints don't disturb xmission
}

float LevelDriver::ReadAccel(Axes axis)
{
	return values[axis];
}

void LevelDriver::Update()
{
	SetupSPI();

	unsigned char dataXH = ReadData(OutX_H);
	unsigned char dataXL = ReadData(OutX_L);
	unsigned char dataYH = ReadData(OutY_H);
	unsigned char dataYL = ReadData(OutY_L);
	unsigned char dataZH = ReadData(OutZ_H);
	unsigned char dataZL = ReadData(OutZ_L);

	short dataX = ((unsigned short)dataXH << 8) | dataXL;
	short dataY = ((unsigned short)dataYH << 8) | dataYL;
	short dataZ = ((unsigned short)dataZH << 8) | dataZL;
	
	values[XAxis] = dataX;
	values[YAxis] = dataY;
	values[ZAxis] = dataZ;  
}

unsigned short LevelDriver::WriteWord(unsigned short data)
{
	Uint16 rw = data; // leave as is (16 bit)
	SpiaRegs.SPITXBUF=rw; // send data
	
	//wait for end of transfer
	while( SpiaRegs.SPISTS.bit.INT_FLAG != 1 );
	// read data
	unsigned char rd = SpiaRegs.SPIRXBUF & 0xFFFF;

	return rd;
}

void LevelDriver::WriteData(Registers regaddr, unsigned char data)
{
	GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1; // select
	
	// send data
	unsigned short dataToWrite = ((unsigned short)regaddr << 8) | data;
	WriteWord(dataToWrite);
	
	GpioDataRegs.GPBSET.bit.GPIO52 = 1; // deselect
}
 
unsigned char LevelDriver::ReadData(Registers regaddr)
{
	GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1; // select
	
	// send data
	unsigned short dataToWrite = 0x008000 | ((unsigned short)regaddr << 8) | 0x0FF; // 0x8000 - write flag
	// get data
	unsigned char readData = WriteWord(dataToWrite) & 0x0FF;

	GpioDataRegs.GPBSET.bit.GPIO52 = 1; // deselect

	return readData;
}
